// -*- c++ -*-
// Copyright 2008 Isis Innovation Limited

#ifndef __CAMERACALIBRATOR_H
#define __CAMERACALIBRATOR_H
#include "CalibImage.h"
#include <gvars3/gvars3.h>
#include <vector>
#include <opencv/cv.h>

#include "GLWindow2.h"

#include "System_ros.h"
#include "ros/ros.h"
#include "sensor_msgs/Image.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/NavSatFix.h"
#include <tf/transform_listener.h>
#include "geometry_msgs/Quaternion.h"


class CameraCalibrator
{
public:
  CameraCalibrator();
  ~CameraCalibrator();
  bool Stop();
  void NewFrame(const sensor_msgs::Image::ConstPtr &msg);
  void NewImage(const std::string filename);
  void OptimizeImages();

protected:
  void Reset();

  void HandleFrame(CVD::Image<CVD::byte> imFrame);
  static void MainLoopCallback(void* pvUserData);
  void MainLoopStep();
  

  IplImage *mpIplImage;
  CVD::ImageRef mirFrameSize;
  CVD::Image<CVD::Rgb<CVD::byte> > mimFrameRGB;
  CVD::Image<CVD::byte> mimFrameBW;
  GLWindow2 *mpGLWindow;
  ATANCamera mCamera;
  bool mbDone;

  std::vector<CalibImage> mvCalibImgs;
  void OptimizeOneStep();
  
  bool mbGrabNextFrame;
  GVars3::gvar3<int> mgvnOptimizing;
  GVars3::gvar3<int> mgvnShowImage;
  GVars3::gvar3<int> mgvnDisableDistortion;
  GVars3::gvar3<int> mgvnCameraWidth;
  GVars3::gvar3<int> mgvnCameraHeight;
  GVars3::gvar3<int> mgvnCameraDepth;
  GVars3::gvar3<int> mgvnCameraChannels;
  GVars3::gvar3<int> mgvnCameraFrameRate;

  double mdMeanPixelError;

  void GUICommandHandler(std::string sCommand, std::string sParams);
  static void GUICommandCallBack(void* ptr, std::string sCommand, std::string sParams);
};

#endif
